#include <cstring>
#include <opencv2/opencv.hpp>
#include <geometry_msgs/Twist.h> 

#include "basic/tools.h"
#include "ros/ros1node.h"

ROS1Node::ROS1Node(std::shared_ptr<ros::NodeHandle> nh, std::string ipv4, uint16_t port):_nh(nh) {
    isNavState = false;
    _re = std::make_shared<Json::Reader>();
    // UDP Client
    _updClient = std::make_shared<UDPClient>(ipv4, port);
    // 发布
    pub_vel = _nh->advertise<geometry_msgs::Twist>("/smoother_cmd_vel", 10);
    ac = std::make_shared<MoveBaseClient>("move_base", true);
    pub_initPose = _nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1);
    pub_goal = _nh->advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1000);
    // 订阅
    sub_battery = _nh->subscribe("/bms_fb", 10, &ROS1Node::battery_cb, this);
    sub_odom = _nh->subscribe("/robot_pose_ekf/ekf_odom", 10, &ROS1Node::odometry_cb, this);
    sub_occup = _nh->subscribe("/map", 1000000, &ROS1Node::occup_cb, this);
}

void ROS1Node::thread_UDPRecv() {
    _updClient->get_udp_obj()->onMessageReceived = [&](std::string message, std::string ipv4, uint16_t port) {
        std::unique_lock<std::mutex> lock(_mtx_message);
        if (!message.empty()) {
            _message = message;
            bool bRet = _re->parse(message, _va);
            // lock.unlock();
            if (false == bRet) {
                LOG(WARNING) << "[ROS1Node] json parse error";
            } else {
                _cond.notify_one();
            }
            message = "";
        }
        // LOG(INFO) << "[ROS1Node]" << ipv4 << ":" << port << "->" << _message;
    };
}

void ROS1Node::thread_op() {
    std::unique_lock<std::mutex> msg_lock(_mtx_message);
    _cond.wait(msg_lock, [this]{return !_va["cmd"].isNull();});
    msg_lock.unlock();
    if (false == _va["cmd"].isInt()) {
        LOG(WARNING) << "[Robot] Mode is not Int";
    } else {
        int cmd = _va["cmd"].asInt();
        launch_cmd(cmd);
        // LOG(INFO) << "cmdcmdcmdcmdcmdcmd: " << cmd;
    }
    // std::lock_guard<std::mutex> va_lock(_mtx_message);
     _va.clear();
}

void ROS1Node::thread_UDPSend() {
    std::unique_lock<std::mutex> upd_lock(_mtx_udp);
    root.clear();
    unsigned int timestamp = long(ros::Time::now().toSec() * 1e3) & 0xFFFF;
    root["upload"] = Json::Value(1);
    root["timestamp"] = Json::Value(float(timestamp));    
    root["crc"] = Json::Value(float(generateCRC(timestamp)));
    if (bBattery == true) {
        root["battry"] = Json::Value(_remaining_capacity);
        bBattery = false;
    }
    if (bOdometry == true) {
        root["pose"]["position"]["x"] = Json::Value(_pose[0]);
        root["pose"]["position"]["y"] = Json::Value(_pose[1]);
        root["pose"]["position"]["z"] = Json::Value(_pose[2]);
        root["pose"]["linear"]["x"]   = Json::Value(_pose[3]);
        root["pose"]["linear"]["y"]   = Json::Value(_pose[4]);
        root["pose"]["linear"]["z"]   = Json::Value(_pose[5]);
        root["pose"]["euler"]["yaw"]  = Json::Value(_pose[6]);
        bOdometry = false;
    }
    if (bGoal == true) {
        auto state = ac->getState(); // actionlib::SimpleClientGoalState
        GoalState = state.toString().c_str();
        root["goal"] = Json::Value(GoalState);
        bGoal = false;
        if(state == actionlib::SimpleClientGoalState::SUCCEEDED 
            || state == actionlib::SimpleClientGoalState::LOST) {
            bGoal = false;
        }
    }
    if (bSaveMap == true) {
        root["savemapname"] = Json::Value(_map.filename);
        bGoal = false;
    }

    std::string msg = _styled_writer.write(root);
    _updClient->UDPClientSend(msg);
}

void ROS1Node::launch_cmd(int cmd) {
   if (cmd == (int)CMD::SaveMap) {
        std::unique_lock<std::mutex> map_lock(_mtx_map);
        bSaveMap = true;
        std::string filename = this->get_map_path() + _va["mapname"].asString();
        std::string cmd = ROS_CMD[0] +  filename;
        std::string mapname = filename + ".pgm";
        _map.filename = filename + ".yaml";
        pid_t pid = fork();
        if (pid < 0) {
            LOG(ERROR) << "[ROS1Node] SaveMap pid start error";
            std::exit(EXIT_FAILURE);
        }
        if (pid == 0) {
            LOG(INFO) << "[ROS1Node] SaveMap pid  start";
            std::system(cmd.c_str());
            std::exit(EXIT_SUCCESS);
        }
        usleep(2000);
        while (!isFileExists(mapname)) {
            // sleep(1);
            usleep(50000);  // 50 ms
        }
        curl_image(mapname, _map.filename);
    } else if (cmd == (int)CMD::VelContol) {
        float vel = _va["linear"]["x"].asFloat();
        float yaw = _va["angular"]["z"].asFloat();
        publish_vel(vel, yaw);
    } else if (cmd == (int)CMD::InitPose) {
        float x = _va["position"]["x"].asFloat();
        float y = _va["position"]["y"].asFloat();
        float yaw = _va["angular"]["z"].asFloat();
        publish_init_pose(x, y, yaw);
    } else if (cmd == (int)CMD::SetGoal) {
        float x = _va["position"]["x"].asFloat();
        float y = _va["position"]["y"].asFloat();
        float yaw = _va["angular"]["z"].asFloat();
        publish_goal(x, y, yaw);
    } else if (cmd == (int)CMD::StopNavi) {
        // 取消正在动作服务器上运行的所有目标。
        ac->cancelAllGoals();
        LOG(INFO) << "cmd555555555555555555";
        // 取消我们当前正在追踪的的目标。   
        // ac->cancelGoal();
        // 取消在指定时间之前和之前标记的所有目标。
        // ac->cancelGoalsAtAndBeforeTime (const ros::Time &time)
    } 
}

void ROS1Node::publish_vel(float vel, float yaw) {
    geometry_msgs::Twist vel_cmd; 
    vel_cmd.linear.x = vel; 
    vel_cmd.linear.y = 0; 
    vel_cmd.linear.z = 0; 
    vel_cmd.angular.x = 0; 
    vel_cmd.angular.y = 0; 
    vel_cmd.angular.z = yaw;
    pub_vel.publish(vel_cmd); 
}

void ROS1Node::publish_goal(float x, float y, float yaw) {
    // pid_t pid = fork();
    // if (pid < 0) {
    //     LOG(ERROR) << "[Robot] publish_goal pid start error";
    //     std::exit(EXIT_FAILURE);
    // }
    // if (pid == 0) {
    //     LOG(INFO) << "[Robot] publish_goal pid start";
    //     std::string prefix = "rosrun ros_midware simple_goal ";
    //     std::stringstream sstream;
    //     sstream << prefix << x << " " << y << " " << yaw;
    //     std::string cmd = sstream.str();
    //     std::system(cmd.c_str());
    //     std::exit(EXIT_SUCCESS);
    // }
    
    while (!ac->waitForServer(ros::Duration(5.0))) {
        LOG(INFO) << "[ROS1Node] Waiting for the move_base action server to come up";
    }
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "base_link";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position.x = x;
    goal.target_pose.pose.position.y = y;
    goal.target_pose.pose.orientation.z = std::sin(yaw * 0.5);
    goal.target_pose.pose.orientation.w = std::cos(yaw * 0.5);
    LOG(INFO) << "[ROS1Node] Sending goal";
    ac->sendGoal(goal);
    // ac->waitForResult();
    bGoal = true;
    // auto state = ac->getState(); // actionlib::SimpleClientGoalState
    // GoalState = state.toString().c_str();
    // if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
    //     GoalState = true;
    // } else {
    //     GoalState = false;
    // }

// ResultConstPtr getResult ()
// 获取当前目标的结果
}

// //发布导航目标点信息
// void ROS1Node::publish_goal(float x, float y, float yaw) {
//   geometry_msgs::PoseStamped goal;
//   //设置frame
//   goal.header.frame_id = "base_link";
//   //设置时刻
//   goal.header.stamp = ros::Time::now();
//   goal.pose.position.x = x;
//   goal.pose.position.y = y;
//   goal.pose.position.z = 0;
//   goal.pose.orientation.z = std::sin(yaw * 0.5);
//   goal.pose.orientation.w = std::cos(yaw * 0.5);
//   pub_goal.publish(goal);
// }

void ROS1Node::publish_init_pose(float x, float y, float yaw) {
    isNavState = true;

    geometry_msgs::PoseWithCovarianceStamped msg;
    msg.header.frame_id = "map";
    msg.pose.pose.position.x = x;
    msg.pose.pose.position.y = y;
    msg.pose.covariance[0] = 0.25;
    msg.pose.covariance[6 * 1 + 1] = 0.25;
    msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
    msg.pose.pose.orientation.z = std::sin(yaw * 0.5);
    msg.pose.pose.orientation.w = std::cos(yaw * 0.5);
    pub_initPose.publish(msg);
}

void ROS1Node::battery_cb(const yhs_msgs::bms_fb msg) {
    // 解析电量 发送 UDP
    std::lock_guard<std::mutex> odom_lock(_mtx_udp);
    _remaining_capacity = msg.bms_fb_remaining_capacity;
    bBattery = true;
}

void ROS1Node::odometry_cb(const nav_msgs::Odometry::ConstPtr &msg) {
    std::lock_guard<std::mutex> odom_lock(_mtx_udp);
    if (isNavState == false) {
        // 位置
        _pose[0] = msg->pose.pose.position.x;
        _pose[1] = msg->pose.pose.position.y;
        _pose[2] = msg->pose.pose.position.z;
        // 速度
        _pose[3] = msg->twist.twist.linear.x;
        _pose[4] = msg->twist.twist.linear.y;
        _pose[5] = msg->twist.twist.linear.z;
        // 四元数转换YAW
        float qx = msg->pose.pose.orientation.x;
        float qy = msg->pose.pose.orientation.y;
        float qz = msg->pose.pose.orientation.z;
        float qw = msg->pose.pose.orientation.w;
        float siny_cosp = 2 * (qw * qz + qx * qy);
        float cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
        _pose[6] = std::atan2(siny_cosp, cosy_cosp);
    } else {
        try {
            listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
        }   
        catch (tf::TransformException &ex) {
            LOG(ERROR) << ex.what();
        }
        _pose[0] = static_cast<double>(transform.getOrigin().x());
        _pose[1] = static_cast<double>(transform.getOrigin().y());
        _pose[6] = tf::getYaw(transform.getRotation());
    }
    bOdometry = true;
}

void ROS1Node:: occup_cb(const nav_msgs::OccupancyGrid::ConstPtr &msg) {
    std::lock_guard<std::mutex> odom_lock(_mtx_occupy);
    if (++bOccup == nOCcup) {
        
        std::string filename = this->get_map_path() + "temp";
        std::string cmd = ROS_CMD[0] +  filename;
        std::string mapname = filename + ".pgm";
        pid_t pid = fork();
        if (pid < 0) {
            LOG(ERROR) << "[ROS1Node] occup_cb pid start error";
            std::exit(EXIT_FAILURE);
        }
        if (pid == 0) {
            LOG(INFO) << "[ROS1Node] occup_cb pid  start";
            std::system(cmd.c_str());
            std::exit(EXIT_SUCCESS);
        }
        usleep(30000);
        while (!isFileExists(mapname)) {
            // sleep(1);
            usleep(30000);  // 30 ms
        }
        curl_image(mapname, filename + ".yaml");

    //     _occupy.width = msg->info.width;
    //     _occupy.height = msg->info.height;
    //     _occupy.resolution = msg->info.resolution;
    //     _occupy.origin_x = msg->info.origin.position.x;
    //     _occupy.origin_y = msg->info.origin.position.y;
    //     _occupy.origin_z = msg->info.origin.position.z;
    //     _occupy.quat_x = msg->info.origin.orientation.x;
    //     _occupy.quat_y = msg->info.origin.orientation.y;
    //     _occupy.quat_z = msg->info.origin.orientation.z;
    //     _occupy.quat_w = msg->info.origin.orientation.w;

    //     // LOG(INFO) << "width:" <<  _occupy.width << " , height:" << _occupy.height;
    //     // 写入像素点
       
    //     _map.filename = this->get_map_path() + "temp.jpg";
    //     cv::Mat image = cv::Mat::zeros(_occupy.width, _occupy.height, CV_8UC1);
    //     for(int i = 0; i < _occupy.height; i++) {
    //         for(int j = 0; j < _occupy.width; j++) {
    //             int value = msg->data[i * _occupy.width + j];
    //             uchar pixel = 0;
    //             if (value == 100) {
    //                 pixel = 0;      // black
    //             } else if (value == 0) {
    //                 pixel = 255;    // white
    //             } else if (value == -1) {
    //                 pixel = 128;    // gray
    //             }
    //             image.at<uchar>(j, i) = pixel;
    //         }
    //     }

        // cv::Mat image = cv::Mat::zeros(_occupy.width, _occupy.height, CV_8UC1);
        // for(int i = 0; i < _occupy.height; i++) {
        //     for(int j = 0; j < _occupy.width; j++) {
        //         int value = msg->data[i * _occupy.width + j];
        //         uchar pixel = 0;
        //         if (value == 100) {
        //             pixel = 0;      // black
        //         } else if (value == 0) {
        //             pixel = 255;    // white
        //         } else if (value == -1) {
        //             pixel = 128;    // gray
        //         }
        //         image.at<uchar>(j, i) = pixel;
        //     }
        // }

        // for (int i = 0; i < msg->data.size(); i++) {
        //     int x = i % _occupy.width;
        //     int y = (int)i / _occupy.width;
        //     uchar pixel = 0;
        //     if (msg->data[i] == 100) {
        //         pixel = 0;      // black
        //     } else if (msg->data[i] == 0) {
        //         pixel = 255;    // white
        //     } else if (msg->data[i] == -1) {
        //         pixel = 128;    // gray
        //     }
        //     std::cout << msg->data[i] << " ";
        //     image.at<uchar>(x, y) = pixel;
        // }   
        // std::cout<< std::endl;


        // cv::imwrite(_map.filename, image);
        // usleep(1000);
        // // while (!isFileExists(_map.filename)) {
        // //     // sleep(1);
        // //     usleep(200000);  // 200 ms
        // // }
        bOccup = 0;
    }
}

void ROS1Node::stop() {
    std::system("killall -9 roscore");
    std::system("killall -9 rosmaster");
    std::system("killall -9 simple_goal");
    std::system("killall -9 run_ros1");
    // execlp
}